// --------------------------------------------------------------------------------------------------------------------
// <copyright file="CubicSpline.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using System.Text;

    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Represents a cubic spline
    /// </summary>
    [DataContract]
    public class CubicSpline
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="CubicSpline"/> class.
        /// </summary>
        /// <param name="numDOF">Number of degrees of freedom.</param>
        /// <param name="numPoints">Number of spline points.</param>
        public CubicSpline(
            int numDOF,
            int numPoints)
        {
            if (numDOF <= 0)
            {
                throw new ArgumentException("numDOF must be greater than zero");
            }

            if (numPoints <= 1)
            {
                throw new ArgumentException("numPoints must be greater than one");
            }

            this.NumDOF = numDOF;
            this.NumPoints = numPoints;
            this.Times = new double[numPoints];
            this.TargetPositions = new double[numDOF, numPoints];
            this.TargetVelocities = new double[numDOF, numPoints];
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="CubicSpline"/> class.
        /// </summary>
        /// <param name="numDOF">Number of degrees of freedom.</param>
        /// <param name="numPoints">Number of spline points.</param>
        /// <param name="times">Spline point times.  Must be monotonically increasing and first value must be zero.</param>
        /// <param name="targetPositions">Spline point positions indexed by [DOF, waypoint #].</param>
        /// <param name="targetVelocities">Spline point velocities indexed by [DOF, waypoint #].</param>
        public CubicSpline(
            int numDOF,
            int numPoints,
            double[] times,
            double[,] targetPositions,
            double[,] targetVelocities)
        {
            if (numDOF <= 0)
            {
                throw new ArgumentException("numDOF must be greater than zero");
            }

            if (numPoints <= 1)
            {
                throw new ArgumentException("numPoints must be greater than one");
            }

            if (targetPositions.GetLength(0) != numDOF)
            {
                throw new ArgumentException(
                    string.Format(
                    "numDOF is {0}, but targetPositions contains {1} DOFs",
                        numDOF,
                        targetPositions.GetLength(0)));
            }

            if (targetVelocities.GetLength(0) != numDOF)
            {
                throw new ArgumentException(
                    string.Format(
                    "numDOF is {0}, but targetVelocities contains {1} DOFs",
                        numDOF,
                        targetVelocities.GetLength(0)));
            }

            if (targetPositions.GetLength(1) != numPoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "numPoints is {0}, but targetPositions contains {1} points",
                        numPoints,
                        targetPositions.GetLength(1)));
            }

            if (targetVelocities.GetLength(1) != numPoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "numPoints is {0}, but targetVelocities contains {1} points",
                        numPoints,
                        targetVelocities.GetLength(1)));
            }

            if (times.Length != numPoints)
            {
                throw new ArgumentException(
                    string.Format(
                    "numPoints is {0}, but times contains {1} points",
                        numPoints,
                        times.Length));
            }

            if (times[0] != 0)
            {
                throw new ArgumentException(
                    string.Format(
                    "time of first point must be 0, found {0}",
                        times[0]));
            }

            this.NumDOF = numDOF;
            this.NumPoints = numPoints;
            this.Times = times;
            this.TargetPositions = targetPositions;
            this.TargetVelocities = targetVelocities;
        }

        /// <summary>
        /// Gets or sets number of degrees of freedom the spline controls
        /// </summary>
        [DataMember]
        public int NumDOF { get; set; }

        /// <summary>
        /// Gets or sets number of points in the spline.
        /// </summary>
        [DataMember]
        public int NumPoints { get; set; }

        /// <summary>
        /// Gets or sets a list of times in seconds indicating the target time of a spline point.
        /// For example, duration[n] is the amount of time in seconds from the start of the 
        /// spline to the point where it should reach target position n.
        /// </summary>
        [DataMember]
        public double[] Times { get; set; }

        /// <summary>
        /// Gets or sets target positions for each point in spline.  targetPositions[D, N] is the target position for degree of
        /// freedom D at point N.
        /// </summary>
        [DataMember]
        public double[,] TargetPositions { get; set; }

        /// <summary>
        /// Gets or sets target velocities for each point in spline.  targetVelocity[D, N] is the target velocity for degree of freedom
        /// D at point N.  Must be zero for last point in spline.  Strongly recommended to be zero at any spline point where
        /// degree of freedom changes direction.
        /// </summary>
        [DataMember]
        public double[,] TargetVelocities { get; set; }

        /// <summary>
        /// Gets or sets the position of each degree of freedom at the specified time.
        /// </summary>
        /// <param name="time">Time since start of spline.</param>
        /// <returns>A array of positions.</returns>
        public double[] GetPositions(double time)
        {
            double[] positions = new double[this.NumDOF];

            // time == 0 is a special case, as it is the only allowable situation where
            // nextPoint == 0
            if (time == 0)
            {
                for (int i = 0; i < this.NumDOF; ++i)
                {
                    positions[i] = this.TargetPositions[i, 0];
                }

                return positions;
            }

            // basic steps for each DOF are 
            // 1. find the current segment (e.g. what two points are we between).  This is the same for each DOF since
            //     points are constrained to be placed at same locations in time for all DOFs
            // 2. compute the coefficients for the segment (can improve efficiency by implementing caching for this step) 
            // 3. comput position target for DOF
            int nextPoint = this.FindNextPoint(time);

            // check if we are off the end of the spline
            // return null if that is the case
            if (nextPoint == -1)
            {
                return null;
            }

            // currently don't allow first point to have a time other than zero (except in case above)
            // so we should never get a result for 'nextPoint' of 0.
            if (nextPoint <= 0)
            {
                throw new ApplicationException(string.Format("Invalid time for first point, expected a value > 0, found {0}", this.Times[0]));
            }

            // shift time to be relative to time of previous point
            double tf = this.Times[nextPoint] - this.Times[nextPoint - 1];
            double t = time - this.Times[nextPoint - 1];

            // spline coef. computation.  See Ch. 7 of "Introduction to Robotics"
            // by J. Craig.
            for (int i = 0; i < this.NumDOF; ++i)
            {
                double a0 = this.TargetPositions[i, nextPoint - 1];
                double a1 = this.TargetVelocities[i, nextPoint - 1];
                double a2 =
                    (3.0 * (this.TargetPositions[i, nextPoint] - this.TargetPositions[i, nextPoint - 1])) / Math.Pow(tf, 2)
                    - (2.0 * this.TargetVelocities[i, nextPoint - 1]) / tf
                    - this.TargetVelocities[i, nextPoint] / tf;
                double a3 =
                     -(2.0 * (this.TargetPositions[i, nextPoint] - this.TargetPositions[i, nextPoint - 1])) / Math.Pow(tf, 3)
                     + (this.TargetVelocities[i, nextPoint] + this.TargetVelocities[i, nextPoint - 1]) / Math.Pow(tf, 2);

                positions[i] = a0 + a1 * t + a2 * Math.Pow(t, 2) + a3 * Math.Pow(t, 3);
            }

            return positions;
        }

        /// <summary>
        /// Returns the index of the waypoint at the beginning of the segment
        /// that spans the indicated time.
        /// </summary>
        /// <param name="time">Time in seconds.</param>
        /// <returns>Index of waypoint at the beginning of the segment that spans the indicated time.  -1 if time is not on the segment.</returns>
        public int GetSplineIndex(double time)
        {
            if (time < 0)
            {
                return -1;
            }

            int point = this.FindNextPoint(time);

            if (point == -1)
            {
                return -1;
            } 

            return point - 1;
        }

        /// <summary>
        /// Sets the velocity reference at intermediate points in order to maintain
        /// C2 continuity at all points.  See pg. 250 of Mathematical Elements for Computer Graphics by Rogers and Adams (2nd ed).
        /// </summary>
        /// <param name="enforceZeroVelocityAtSecondPoint">
        /// Indicates whether or not to enforce zero velocity at both first and second point.  
        /// This is useful for cases where the spline is used for path planning/tracking and the first spline segment is a move to get on the desired path.
        /// </param>
        public void FixIntermediatePointVelocities(bool enforceZeroVelocityAtSecondPoint)
        {
            // Set velocities of first, last and optionally 2 point at each dof
            for (int dof = 0; dof < this.NumDOF; ++dof)
            {
                this.TargetVelocities[dof, 0] = 0;
                this.TargetVelocities[dof, this.NumPoints - 1] = 0;

                if (enforceZeroVelocityAtSecondPoint)
                {
                    this.TargetVelocities[dof, 1] = 0;
                }
            }

            int startPoint;

            if (enforceZeroVelocityAtSecondPoint)
            {
                startPoint = 2;
            }
            else
            {
                startPoint = 1;
            }

            // see http://www.korf.co.uk/spline.pdf for explanation
            // of algorithm
            for (int dof = 0; dof < this.NumDOF; ++dof)
            {
                for (int point = startPoint; point < this.NumPoints - 1; ++point)
                {
                    if (this.IsInflectionPoint(dof, point, this.TargetPositions))
                    {
                        this.TargetVelocities[dof, point] = 0;
                    }
                    else
                    {
                        this.TargetVelocities[dof, point] = 2.0 /
                                            ((this.Times[point + 1] - this.Times[point]) / (this.TargetPositions[dof, point + 1] - this.TargetPositions[dof, point]) +
                                             (this.Times[point] - this.Times[point - 1]) / (this.TargetPositions[dof, point] - this.TargetPositions[dof, point - 1]));
                    }
                }
            }
        } 

        /// <summary>
        /// Returns the index of the next waypoint after given time.
        /// </summary>
        /// <param name="time">Time after start of spline.</param>
        /// <returns>Index of next waypoint.  -1 if time occurs after end of spline.</returns>
        private int FindNextPoint(double time)
        {
            int nextPoint = 0;

            while (nextPoint < this.NumPoints && this.Times[nextPoint] < time)
            {
                ++nextPoint;
            }

            // check if we are off either end of the spline
            if (nextPoint == this.NumPoints)
            {
                return -1;
            }

            return nextPoint;
        }

        /// <summary>
        /// Returns true if the indicate DOF/point combination is an inflection point,
        /// i.e. if the direction of motion changes at this point.
        /// </summary>
        /// <param name="dof">DOF number.</param>
        /// <param name="pointNumber">Point number.</param>
        /// <param name="positions">Two dimensional array of positions.</param>
        /// <returns>True if specified point is an inflection point.</returns>
        private bool IsInflectionPoint(int dof, int pointNumber, double[,] positions)
        {
            if ((positions[dof, pointNumber - 1] <= positions[dof, pointNumber] && positions[dof, pointNumber + 1] <= positions[dof, pointNumber]) ||
                (positions[dof, pointNumber - 1] >= positions[dof, pointNumber] && positions[dof, pointNumber + 1] >= positions[dof, pointNumber]))
            {
                return true;
            }

            return false;
        }
    }
}
